package com.ijiuchuang.jc_localization.ble;

/**
 * Created by Veev on 2016/6/17
 *
 * @link 384506557@qq.com
 */
public class BleFilter {

    /**
     * fn		kalmanFiltingDealRssi
     * brief	对rssi进行卡尔曼滤波
     */
    static double Q = 0.005f;    // 过程方差，反应两个时刻加速度方差
    static double RR = 1.0f;     // 测量方差，反应温度计的测量精度
    static double xhatminus;     // 加速度的先验估计，在k-1时刻，对k时刻加速的做出估计
    static double rssi_pre = 0;  // k-1时刻的采样值
    static double rssi_deal_end;
    static double Pminus;        // 先验估计的方差
    static double P_pre;         // 上一时刻加速度最优估计值的方差
    static double K;             // 卡尔曼增益,反应加速计测量结果与过程模型（即当前时刻与下一时刻加速计相同这一模型）的可信程度
    static double PP;            // 误差方差

    /**
     * fn:		kalmanFilteringDoRssi
     * brief:	对采集的rssi信号进行卡尔曼滤波处理
     * prama:	rssi - 采集到的信号强度
     * return:	rssi_deal_end - 卡尔曼滤波完成的信号强度
     */
    public static double KalmanFiltering(double preRssi, double rssi) {
        if (preRssi >= 0) {
            preRssi = rssi;
            P_pre = 1;
        } else {
            xhatminus = preRssi;
            // 预测的方差为上一时刻加速度最优估计值得方差与过程方差之和
            Pminus = P_pre + Q;
            //卡尔曼增益,反应加速计测量结果与过程模型（即当前时刻与下一时刻加速计相同这一模型）的可信程度
            K = Pminus / (Pminus + RR);
            //结合当前的测量值，对上一时刻的预测进行校正，得到校正后的最优估计，该估计具有最小均方差
            rssi_deal_end = xhatminus + K * (rssi - xhatminus);
            //计算最终估计值得方差
            PP = (1 - K) * Pminus;
            P_pre = PP;
            preRssi = rssi_deal_end;
        }
        return preRssi;
    }

    public static double CommonFiltering(double preRssi, double rssi) {
        return (preRssi + rssi) / 2;
    }
}
